/*
mag_navigation.c
提供磁导航逻辑控制，实现磁导航模式接口
designed by lunanting

----------------------
2020-04-28 13:55:00
create file
----------------------
*/

#include "mag_navigation.h"
#include "bsp.h"
#include "algorithm.h"
#include "../driver/navigation_mag.h"

static MAG_NAVI_class mag_navigation={0};
#ifdef BATTERY_VERSION
aos_mutex_t g_mutex_mag_navigation;
u8 g_only_proport_use = 1;  //初期默认只使用PID中的P进行调速
static float w_error = 0;
PIDtypedef PIDMAG ={0};	    //PID 磁导航句柄
float g_robotspeed = ROBOT_BASESPEED;    //绝对基本速度
int mag_record[MAG_RECORD_LEN] ={0};
//debug view
float error_debug = 0;
float speedl_debug = 0;
float speedr_debug = 0;
float mag_middle_debug = 0;
//
#endif


//int __attribute__((weak)) xp_mag_navigation_init(void){println("mag_navigation_init() undefine");}
//int __attribute__((weak)) xp_mag_navigation_move(s8 dir){println("mag_navigation_move() undefine");}

/*
mag navagation module init
return；
    0=success
*/
#ifdef LINE_VERSION
int xp_mag_navigation_init(void){
    int sta=0;

    //get angle 
    // angle=xp_imu_get_angle();
    if(mag_navigation.init==1)return 0;
    sta=xp_mag_init(2,mag_modbus,115200,6);
    if(sta!=0)return -1;
    sta=xp_wheel_init(2,115200,10);
    if(sta!=0)return -2;
    return 0;
}
#elif defined BATTERY_VERSION

/**
 * @description: 
 * @param {type} 
 * @return: 
 */
static void xp_mag_navigation_run()
{
    u8 flag = 0;
    while (1)
    {   
        if(mag_navigation.dir != 0)
        {
            aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);
            xp_mag_navigation_move(mag_navigation.dir);
            aos_mutex_unlock(&g_mutex_mag_navigation);
            flag =0;
        }
        if((mag_navigation.dir == 0)&&(flag ==0))
        {
            aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);
            xp_wheel_set_wheel_speed(0,0);
            aos_mutex_unlock(&g_mutex_mag_navigation);
            flag = 1;
        }
        aos_msleep(50);
    }
    
}

/**
 * @description: xp_mag_navigation_init  magnetic navigation
 * @param : void
 * @return: 
 *          0 suceess
 *          -1 fail
 */

int xp_mag_navigation_init(void)
{
    int sta=0;
    //get angle 
    // angle=xp_imu_get_angle();
    if(mag_navigation.init==1)
    {
        MAG_LOG("mag_navigation has initialize");
        return 0;
    }
    sta = aos_mutex_new(&g_mutex_mag_navigation);
    if (sta != 0) 
    {
        MAG_LOG("aos_mutex_new(&g_mutex_mag_navigation) error");
        return -1;
    }
    sta = PIDx_init(&PIDMAG);
    if(sta != 0)
    {
        MAG_LOG("PIDx initialization error!! ");
        return -1;
    }
    sta = PIDx_setparam(&PIDMAG,PIDMAG_KP,PIDMAG_KI,PIDMAG_KD);
    if(sta != 0)
    {
        MAG_LOG("PIDx_setparam error! ");
        return -1;
    }
    sta = PIDx_setvalue(&PIDMAG,1);     //设定值为1
    if(sta != 0)
    {
        MAG_LOG("PIDx_setvalue error");
        return -1;
    }
    sta=xp_wheel_init(2,1,115200);
    if(sta != 0)
    {
       MAG_LOG("xp_wheel_init error");
       return -3;
    }
    //  暂时不用磁导航
    sta=xp_mag_init(1,mag_modbus,115200,6);
    if(sta!=0)
    {
        MAG_LOG("xp_mag_init error");
        return -2;
    }
    aos_task_new("state_check_thread",xp_mag_navigation_run,NULL,2100);
    mag_navigation.init = 1;
    return 0;
}

#endif

/*
mag navigation move
param:
    dir:move direction，后退<0,停止=0,前进>0
return:
    0=success
*/
#ifdef LINE_VERSION
int xp_mag_navigation_move(s8 dir){
    MAG_location location={0};
    u16 count=0;
    float angle_rang=100;

    println("come in mag navigation mode~,dir=%d",dir);
    while(xp_mag_location(&location)!=0){
        println("mag location fail,will continue~");   //感应不到磁条
        count++;
        if(count>10){
            println("location fail too long,exit mode");
            return -1;
        }
        aos_msleep(1000);
    }
    println("mag location success !");
    xp_wheel_move(1,dir);
    while(1){
        xp_mag_location(&location);
        if(location.middle>LEFT_RIGHT_RANG){
            if(mag_navigation.turn<1){
                xp_wheel_turn(wheel_right,100);
                if(mag_navigation.turn<0){
                    xp_wheel_turn(wheel_right,100);
                }
                mag_navigation.turn=1;
            }
        }
        else if(location.middle<-LEFT_RIGHT_RANG){
            if(mag_navigation.turn>-1){
                xp_wheel_turn(wheel_left,100);
                if(mag_navigation.turn>0){
                    xp_wheel_turn(wheel_left,100);
                }
                mag_navigation.turn=-1;
            }
        }
        else{
            if(mag_navigation.turn<0){
                xp_wheel_turn(wheel_right,100);
                mag_navigation.turn=0;
            }
            else if(mag_navigation.turn>0){
                xp_wheel_turn(wheel_left,100);
                mag_navigation.turn=0;
            }
        }
        // angle_rang=abs(mag_navigation.angle-xp_imu_get_angle());
        if(mag_navigation.dir==0||angle_rang<10){
            xp_wheel_move(0,0);
            break;
        }
        aos_msleep(100);
    }

    println("AGV stop .exit mag navigation mode~");
}

#elif defined BATTERY_VERSION
/**
 * @description: magnetic navigation move
 * @param  dir <0       back
 *         dir = 0      stop
 *         dir > 0      go forward
 * @return: 
 *          0 success   find it success!
 *          -1 fail 
 */
int xp_mag_navigation_find(s8 dir)
{
    u8 count =100;
    int mag_value = 0;
    MAG_location location={0};
    for(int i =0;i<MAG_RECORD_LEN;i++)
    {
        mag_value = mag_record[i];
        MAG_LOG("i %d mag_value %d ",i,mag_value);
        if(mag_value!=0)
        {
            break;
        }
        
    }
    if(mag_value == 0)
    {
        xp_wheel_set_wheel_speed(0,0);
        mag_navigation.dir = 0;
        MAG_LOG("\n\n\r\n  ERROR!!!!ERROR!!!!ERROR!!!! \n agv car can't fint magnetic guideway /n");
        return -1;
    }
    if(dir>0)
    {
        
        if(mag_value > 0)//上一次记录磁条值为正，agv在磁条左边方向
        {
            xp_wheel_set_wheel_speed(FIND_BIG_SPEED,FIND_SML_SPEED);
            while(count--)
            {
                 xp_mag_location(&location);
                 if(location.mag == 2)
                 {
                    sliding_wind_array(&mag_record[0],MAG_RECORD_LEN,location.middle);
                    MAG_LOG("location.middle %d ",location.middle);
                    return 0;
                 }
                MAG_LOG("NOW Be finding and its %d !!!",count);
                aos_msleep(100);
            }
            return -1;
        }
        else
        {
            xp_wheel_set_wheel_speed(FIND_SML_SPEED,FIND_BIG_SPEED);
            while(count--)
            {
                 xp_mag_location(&location);
                 if(location.mag == 2)
                 {
                    sliding_wind_array(&mag_record[0],MAG_RECORD_LEN,location.middle);
                    MAG_LOG("location.middle %d ",location.middle);
                   return 0;
                 }
                 MAG_LOG("NOW Be finding and its %d !!!",count);
                 aos_msleep(100);
            }
            return -1;
        }
    }
    if(dir<0)
    {   
        if(mag_value > 0)//上一次记录磁条值为正，agv在磁条左边方向
        {
            xp_wheel_set_wheel_speed(-FIND_BIG_SPEED,-FIND_SML_SPEED);
            while(count--)
            {
                 xp_mag_location(&location);
                 if(location.mag == 2)
                 {
                    sliding_wind_array(&mag_record[0],MAG_RECORD_LEN,location.middle);
                    MAG_LOG("location.middle %d ",location.middle);
                    return 0;
                 }
                 MAG_LOG("NOW Be finding and its %d !!!",count);
                 aos_msleep(100);
            }
            return -1;
        }
        else
        {
            xp_wheel_set_wheel_speed(-FIND_SML_SPEED,-FIND_BIG_SPEED);
            while(count--)
            {
                 xp_mag_location(&location);
                 if(location.mag == 2)
                 {
                    sliding_wind_array(&mag_record[0],MAG_RECORD_LEN,location.middle);
                    MAG_LOG("location.middle %d ",location.middle);
                     return 0;
                 }
                 MAG_LOG("NOW Be finding and its %d !!!",count);
                 aos_msleep(100);
            }
            return -1;
        }
    }
}

/**
 * @description: printf some vaule mag_record
 * @param  void 
 * @return: void
 */
void printf_mag_record()
{
    for(int i=0;i<MAG_RECORD_LEN;i++)
    {
        MAG_LOG(" mag_record[%d]:%d \n",i,mag_record[i]);
        
    }
}
/**
 * @description: xp_mag_move_start_or_stop
 * @param  flag:1 start
                0 stop  
 * @return: void
 */
void xp_mag_move_start_or_stop(u8 flag)
{
    if(flag == 0)
    {
        mag_navigation.dir = 0;
    }
    else if(flag == 1)
    {
        mag_navigation.dir = 1;
    }
}
/**
 * @description: xp_mag_get_mag_flag()
 * @param  void  
 * @return: 
 *       mag_navigation.dir; 
 */
int xp_mag_get_mag_flag(void)
{
    return mag_navigation.dir; 
}





/**
 * @description: adaptive pid control
 * @param  void 
 * @return: void
 */
void adaptive_pid_control()
{

}

/**
 * @description: magnetic navigation move
 * @param  dir <0       back  attention:Backing down is not currently supported
 *         dir = 0      stop
 *         dir > 0      go forward
 * @return: 
 */
 
int xp_mag_navigation_move(s8 dir)
{
    MAG_location location={0};
    u16 count=0;
    float robotspeed = g_robotspeed;
    float speed_l = 0;
    float speed_r = 0;
    int ret =0;
    if (dir == 0)
    {
        xp_wheel_set_wheel_speed(0,0);
        w_error = 0;
        return 0;   
    }
    while(xp_mag_location(&location)!=0)
    {
        MAG_LOG("mag location fail,will continue~");   //感应不到磁条
        count++;
        if(count>10){
        MAG_LOG("location fail too long,exit mode and ");
        xp_wheel_set_wheel_speed(0,0);
        return -1;
        }
        aos_msleep(1000);
    } 
    xp_mag_location(&location);
    MAG_LOG("mag location success ! and its middle vaule is %d",location.middle);
    //sliding_wind_array(&mag_record[0],MAG_RECORD_LEN,location.middle);
    //printf_mag_record();
    if(location.mag != 2)//have to find Magnetic guideway
    {
        MAG_LOG("location.mag  %d and can't find the mag signal\n",location.mag);
        xp_wheel_set_wheel_speed(0,0);
        mag_navigation.dir = 0;
       /*
       ret = xp_mag_navigation_find(dir);
       if(ret != 0)
       {
          mag_navigation.dir = 0;
          MAG_LOG("agv car could not find the magnetic navigation ");
          return -1;
       }
       */
    }
    if(dir > 0)
    {
        if((location.middle <= 2)&&(location.middle>=-2))
        {
            w_error = 0;
            PIDMAG.last_error = 0;
            PIDMAG.prev_error = 0;
        }
        else
        {
            if(g_only_proport_use)  //只运行PID中的P环节 初期默认
            {
                PIDMAG.last_error = 0;
                PIDMAG.prev_error = 0;
                PIDMAG.Ki = 0;
                PIDMAG.Kd = 0;
                w_error = PIDx_calc(&PIDMAG,location.middle);   //设定值为0 减去负值出来正值
            }
            else
            {
                w_error = PIDx_calc(&PIDMAG,location.middle);   //设定值为0 减去负值出来正值
            }
            
        }
        /*
            w_error 范围保护
        */
        if(fabs(w_error)>=0.11)
        {
            if(w_error>0)
            {
                w_error = 0.11;
            }
            else
            {
                w_error = -0.11;
            }
        }
        speed_l = robotspeed - w_error;
        speed_r = robotspeed + w_error;
        MAG_LOG("/n middle vaule %d   rrobotspeed %f w_error %f speed_l %f speed_r %f ",location.middle,robotspeed,w_error,speed_l,speed_r);
        xp_wheel_set_wheel_speed(speed_l,speed_r);
        error_debug =w_error;
        speedl_debug = speed_l;
        speedr_debug = speed_r;
        mag_middle_debug = location.middle;
    }
    else if(dir < 0)
    {
        MAG_LOG("dir < 0 Backing down is not currently supported");
        return -1;
        w_error = PIDMAG.Kp *location.middle;
        //w_error = PIDx_calc(&PIDMAG,location.middle);
        speed_l = -robotspeed + w_error;
        speed_r = -robotspeed - w_error;
        xp_wheel_set_wheel_speed(speed_l,speed_r);
        MAG_LOG("/n middle vaule %d   rrobotspeed %f w_error %f speed_l %f speed_r %f ",location.middle,robotspeed,w_error,speed_l,speed_r);
        error_debug =w_error;
        speedl_debug = speed_l;
        speedr_debug = speed_r;
        mag_middle_debug = location.middle;
    }
    return 0;
}


int xp_mag_navigation_b_debug(char *type,char *fun,char *param)
{
    int ret = 0;
    s16 mag_r1 = 0;
    static s8 dir = 0;
    static u8 flag =0;
    if(type == NULL)
    {
        MAG_LOG("type is null error !");
        return 0;
    }
    if(strcmp(type,"mag_nav")== 0)
    {
        if(strcmp(fun,"init")==0)
        {
            ret = xp_mag_navigation_init();
            MAG_LOG("mag navigation init  will start !");
            if(ret != 0)
            {
                MAG_LOG("xp_mag_navigation_init fail! and ret :%d ",ret);
                return 0;
            }
            if((mag_navigation.init == 1)&&(flag == 0))
            {
                MAG_LOG("creat a thread  xp_mag_navigation_run ");
                flag = 1;
            }
            MAG_LOG("mag navigation init  success !");
            return 1;
        }
        if(strcmp(fun,"move")==0)
        {
            dir = strtoul(param, NULL, 10);
            mag_navigation.dir = dir;
            MAG_LOG("mag_navigation.dir is %d ",mag_navigation.dir);
            return 1;
        }
        if(strcmp(fun,"sets")==0)
        { 
            g_robotspeed = atof(param);
            MAG_LOG("g_robotspeed is %f ",g_robotspeed);
            return 1;
        }
        if(strcmp(fun,"setpidp")==0)
        {
            PIDMAG.Kp = atof(param);
            MAG_LOG("PIDMAG.Kp is %f ",PIDMAG.Kp );
            return 1;
        }
        if(strcmp(fun,"setpidi")==0)
        {
            PIDMAG.Ki = atof(param);
            MAG_LOG("PIDMAG.Ki is %f ",PIDMAG.Ki);
            return 1;
        }
        if(strcmp(fun,"setpidd")==0)
        {
            PIDMAG.Kd = atof(param);
            MAG_LOG("PIDMAG.Kd is %f ",PIDMAG.Kd );
            return 1;
        }
        if(strcmp(fun,"read") ==0)
        {
            MAG_LOG("speed %f KP:%f KI:%f KD:%f ",g_robotspeed,PIDMAG.Kp,PIDMAG.Ki,PIDMAG.Kd );
            return 1;
        }
        if(strcmp(fun,"setrecord")== 0)
        {
            mag_r1 = strtoul(param, NULL, 10);
            mag_record[0]= mag_r1; 
            MAG_LOG("   mag_record[0] %d  ", mag_record[0]);
            return 1;
        }
        else
        {
            return 0;
        }
    }
    return 0;
    
}

#endif



/******************************debug and test**********************/
//test
void xp_mag_navi_test(void){
    if(mag_navigation.dir==0)return;
    #if defined BATTERY_VERSION
    xp_mag_navigation_move(mag_navigation.dir);
    #endif
}

/*
mag navigation debug
*/
int xp_mag_navigation_debug(char *type,char *fun,char *param){
    if(strcmp(type,"mag_navi")!=0)return 0;
    if(strcmp(fun,"move")==0){
        s8 dir=atoi(param);
        mag_navigation.dir=dir;
    }
    else{
        return 0;
    }
    return 1;
}







//end of the file
